#!/usr/bin/python3
# import DrEmpower_can as servo

#与大然实物电机交互，订阅traj_calculator的消息

import DrEmpower_can as servo
import serial
import time
import math
servo.uart = serial.Serial('/dev/servo_r',115200,timeout=0.5)
# servo.set_angles([i for i in range(1,13)],[0]*12,speed=10,mode=1,param=10)

angle_des=[0,50,-110]
ID_list = [1,2,3,10]
Angle_list = [0]*4

#for right side 
Angle_list[0] = angle_des[0]
Angle_list[1] = -angle_des[1]
Angle_list[2] = angle_des[1]+angle_des[2]
Angle_list[3] = abs(angle_des[2])-abs(angle_des[1])-90.0+angle_des[2]
#end

#for left side 
# Angle_list[0] = angle_des[0]
# Angle_list[1] = angle_des[1]
# Angle_list[2] = -angle_des[1]-angle_des[2]
Angle_list[3] = -(abs(angle_des[2])+abs(angle_des[1])+90.0+angle_des[2])
#end
servo.set_angles(ID_list,Angle_list,speed=10,mode=1,param=10)
time.sleep(2)

# for i in range(1000):
# 	servo.set_torque(10,0.1)
# 	print(servo.motor_state[9])
# 	time.sleep(1)
angle_cur=[0]*4
for i in range(1000):
	servo.set_torques(ID_list,[0.1]*4)
	for j in range(3):
		angle_cur[j] = servo.motor_state[j][0]
	angle_cur[3] = servo.motor_state[9][0]
	#for right side
	Angle_list[0] = angle_cur[0]
	Angle_list[1] = -angle_cur[1]
	Angle_list[2] = angle_cur[1]+angle_cur[2]
	Angle_list[3] = angle_cur[2]+angle_cur[1]-angle_cur[3]
	#end
	#for left side
	# Angle_list[0] = angle_cur[0]
	# Angle_list[1] = angle_cur[1]
	# Angle_list[2] = -angle_cur[1]-angle_cur[2]
	# Angle_list[3] = -angle_cur[2]-angle_cur[1]+angle_cur[3]
	#end
	print(Angle_list)
	time.sleep(1)